/*
 *   BallFollower.cpp
 *
 *   Author: ROBOTIS
 *
 */

#include <stdio.h>
//#include "ImgProcess.h"
#include "../include/MX28.h"
#include "../include/Head.h"
//#include "Action.h"
#include "../include/Walking.h"
#include "../include/BallFollower.h"
#include "../include/MotionStatus.h"


using namespace Robot;


BallFollower::BallFollower()
{
	m_NoBallMaxCount = 10;
	m_NoBallCount = m_NoBallMaxCount;
	m_KickBallMaxCount = 10;
	m_KickBallCount = 0;

	m_KickTopAngle = -5.0;
	m_KickRightAngle = -30.0;
	m_KickLeftAngle = 30.0;

	m_FollowMaxFBStep = 30.0;
	m_FollowMinFBStep = 5.0;
	m_FollowMaxRLTurn = 35.0;
	m_FitFBStep = 3.0;
	m_FitMaxRLTurn = 35.0;
	m_UnitFBStep = 0.3;
	m_UnitRLTurn = 1.0;

	m_GoalFBStep = 0;
	m_GoalRLTurn = 0;
	m_FBStep = 0;
	m_RLTurn = 0;
	DEBUG_PRINT = true;
	KickBall = 0;



}

BallFollower::~BallFollower()
{
}

void BallFollower::Process(Point2D ball_pos, int pan_degree, int tilt_degree)
{



	if(DEBUG_PRINT == true)
		fprintf(stderr, "\r                                                                               \r");

	if(ball_pos.X == -1.0 || ball_pos.Y == -1.0)
	{
		KickBall = 0;

		if(m_NoBallCount > m_NoBallMaxCount) //initially, both variables are 10
		{
			// cannot find a ball
			m_GoalFBStep = 0;
			m_GoalRLTurn = 0;
			//			Head::GetInstance()->MoveToHome();

			if(DEBUG_PRINT == true)
				fprintf(stderr, "[NO BALL]");
		}
		else
		{
			m_NoBallCount++;
			if(DEBUG_PRINT == true)
				fprintf(stderr, "[NO BALL COUNTING(%d/%d)]", m_NoBallCount, m_NoBallMaxCount);
		}
	}
////////////////////
	////////////////////ball is in view
	else
	{
		m_NoBallCount = 0;		

		double pan = pan_degree;//TODO for manual control //MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_PAN);
		double pan_range = 80; //this is what you will find in config.ini // Head::GetInstance()->GetLeftLimitAngle();
		double pan_percent = pan_degree / pan_range;

		double tilt = tilt_degree;//TODO for manual control //MotionStatus::m_CurrentJoints.GetAngle(JointData::ID_HEAD_TILT);
		double tilt_min = -68;//this is what you will find in config.ini //Head::GetInstance()->GetBottomLimitAngle();
		double tilt_range = 0; //this is what you will find in config.ini //Head::GetInstance()->GetTopLimitAngle() - tilt_min;
		double tilt_percent = (tilt - tilt_min) / tilt_range;
		if(tilt_percent < 0){
			tilt_percent = -tilt_percent;
		}

		//TODO to maually control walking based on tilt_degree and pan_degree:
		//1. set pan between m_KickRightAngle and m_KickLeftAngle (-30 and 30 degrees)
		//2. set tilt greater than tilt_min + MX28::RATIO_VALUE2ANGLE (-68 degrees)
////////////////////
////////////////////ball is within kicking field of view
		if(pan > m_KickRightAngle && pan < m_KickLeftAngle){
	////////////////////
	////////////////////TODO ball is close enough to kick
			if(tilt <= (tilt_min + MX28::RATIO_VALUE2ANGLE))
			{
//				if(ball_pos.Y < m_KickTopAngle)
//				{
//					m_GoalFBStep = 0;
//					m_GoalRLTurn = 0;
//					////////////////////
//					////////////////////We should Kick the ball now
//					if(m_KickBallCount >= m_KickBallMaxCount)
//					{
//						m_FBStep = 0;
//						m_RLTurn = 0;
//						if(DEBUG_PRINT == true)
//							fprintf(stderr, "[KICK]");
//						////////////////////
//							////////////////////decide which foot to kick with
//						if(pan > 0)
//						{
//							KickBall = 1; // Left
//							if(DEBUG_PRINT == true)
//								fprintf(stderr, " Left");
//						}
//						else
//						{
//							KickBall = -1; // Right
//							if(DEBUG_PRINT == true)
//								fprintf(stderr, " Right");
//						}
//					}
//					else
//					{
//						KickBall = 0;
//						if(DEBUG_PRINT == true)
//							fprintf(stderr, "[KICK COUNTING(%d/%d)]", m_KickBallCount, m_KickBallMaxCount);
//					}
//				}
//				else
//				{
//					m_KickBallCount = 0;
//					KickBall = 0;
//					m_GoalFBStep = m_FitFBStep;
//					m_GoalRLTurn = m_FitMaxRLTurn * pan_percent;
//					if(DEBUG_PRINT == true)
//						fprintf(stderr, "[FIT(P:%.2f T:%.2f>%.2f)]", pan, ball_pos.Y, m_KickTopAngle);
//				}
			}
		////////////////////
		////////////////////ball is not close enough to kick, we must follow

			//TODO determines front/back distance based head tilt
			//ball is  out in front of robot, concurrent walking and turning may be used

			else
			{
				m_KickBallCount = 0;
				KickBall = 0;
				m_GoalFBStep = m_FollowMaxFBStep * tilt_percent;
				if(m_GoalFBStep < m_FollowMinFBStep){
					m_GoalFBStep = m_FollowMinFBStep;
				}
				m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent;
				if(DEBUG_PRINT == true)
					fprintf(stderr, "[FOLLOW(P:%.2f T:%.2f>%.2f]", pan, tilt, tilt_min);
			}
		}

		////////////////////
		////////////////////ball is not in kick field of view, lets turn so it will be

		//TODO determines left/right distance based head pan
		//ball is outside of the kicking region, must concentrate on turning, not walking forward
		else
		{
			m_KickBallCount = 0;
			KickBall = 0;
			m_GoalFBStep = 0;
			m_GoalRLTurn = m_FollowMaxRLTurn * pan_percent; //pan_percent is 1 or -1 at most
			if(DEBUG_PRINT == true)
				fprintf(stderr, "[TURN_FOLLOW(P:%.2f T:%.2f>%.2f]", pan, tilt, tilt_min);
		}
	}





		if(m_GoalFBStep == 0 && m_GoalRLTurn == 0 && m_FBStep == 0 && m_RLTurn == 0)
		{
			if(Walking::GetInstance()->IsRunning() == true)
				Walking::GetInstance()->Stop();
			else
			{
				if(m_KickBallCount < m_KickBallMaxCount)
					m_KickBallCount++;
			}

			if(DEBUG_PRINT == true)
				fprintf(stderr, " STOP");
		}

		else
		{
			if(DEBUG_PRINT == true)
				fprintf(stderr, " START\n");

			if(Walking::GetInstance()->IsRunning() == false)
			{
				m_FBStep = 0;
				m_RLTurn = 0;
				m_KickBallCount = 0;
				KickBall = 0;
				Walking::GetInstance()->X_MOVE_AMPLITUDE = m_FBStep;
				Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn;
				Walking::GetInstance()->Start();
			}
			else
			{


				if(m_FBStep < m_GoalFBStep)
					m_FBStep += m_UnitFBStep;
				else if(m_FBStep > m_GoalFBStep)
					m_FBStep = m_GoalFBStep;
				Walking::GetInstance()->X_MOVE_AMPLITUDE = m_FBStep;

				if(m_RLTurn < m_GoalRLTurn)
					m_RLTurn += m_UnitRLTurn;
				else if(m_RLTurn > m_GoalRLTurn)
					m_RLTurn -= m_UnitRLTurn;
				Walking::GetInstance()->A_MOVE_AMPLITUDE = m_RLTurn;

				if(DEBUG_PRINT == true){
					fprintf(stderr, " (FB:%.1f RL:%.1f)\n", m_FBStep, m_RLTurn);
					// printf("IM WALKING!!!!!\n");
				}
			}
		}
	}

